#include "Int_MPU6050.h"
extern int16_t z_accel_static;
int32_t MPU6050_Acc_Offset[6] = {0};
// 实现MPU6050的写一个字节
void Int_MPU6050_WriteByte(uint8_t reg, uint8_t data)
{
    HAL_I2C_Mem_Write(&hi2c1, MPU_IIC_ADDR_W, reg, 1, &data, 1, 1000);
}
// 实现MPU6050的读一个字节
uint8_t Int_MPU6050_ReadByte(uint8_t reg)
{
    uint8_t data;
    HAL_I2C_Mem_Read(&hi2c1, MPU_IIC_ADDR_R, reg, 1, &data, 1, 1000);
    return data;
}
// 实现MPU6050的读多个字节
void Int_MPU6050_ReadBytes(uint8_t reg, uint8_t *buf, uint8_t len)
{
    HAL_I2C_Mem_Read(&hi2c1, MPU_IIC_ADDR_R, reg, 1, buf, len, 1000);
}
// 实现MPU6050的写多个字节
void Int_MPU6050_WriteBytes(uint8_t reg, uint8_t *buf, uint8_t len)
{
    HAL_I2C_Mem_Write(&hi2c1, MPU_IIC_ADDR_W, reg, 1, buf, len, 1000);
}
void Int_MPU6050_Calibration(void);
// 初始化MPU6050
void Int_MPU6050_Init(void)
{
    // 重启
    Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x18);
    vTaskDelay(200);
    // 关闭睡眠模式,并设置时钟源
    Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x01);
    // 配置寄存器
    Int_MPU6050_WriteByte(MPU_CFG_REG, 0x01);
    // 陀螺仪配置寄存器
    Int_MPU6050_WriteByte(MPU_GYRO_CFG_REG, 0x18);
    // 加速度计配置寄存器
    Int_MPU6050_WriteByte(MPU_ACCEL_CFG_REG, 0x00);
    // 设置低通滤波频率
    Int_MPU6050_WriteByte(MPU_SAMPLE_RATE_REG, 0x01);
    Int_MPU6050_WriteByte(MPU_CFG_REG, 0x02);
    // 使能传感器
    Int_MPU6050_WriteByte(MPU_PWR_MGMT2_REG, 0x00);

    // 进行零偏校准
    Int_MPU6050_Calibration();
    // z_accel_static = Com_IMU_GetNormAccZ();

}
void Int_MPU6050_Read_Gyro(Gyro_Struct *gyro)
{
    uint8_t buf[6];
    Int_MPU6050_ReadBytes(MPU_GYRO_XOUTH_REG, buf, 6);
    gyro->x = (buf[0] << 8) | buf[1];
    gyro->y = (buf[2] << 8) | buf[3];
    gyro->z = (buf[4] << 8) | buf[5];
}
void Int_MPU6050_Read_Accel(Accel_Struct *accel)
{
    uint8_t buf[6];
    Int_MPU6050_ReadBytes(MPU_ACCEL_XOUTH_REG, buf, 6);
    accel->x = (buf[0] << 8) | buf[1];
    accel->y = (buf[2] << 8) | buf[3];
    accel->z = (buf[4] << 8) | buf[5];
}
// 读取MPU6050数据
void Int_MPU6050_Read_Gyro_Accel(GyroAccel_Struct *gyro_accel)
{

    Int_MPU6050_Read_Gyro(&(gyro_accel->gyro));
    Int_MPU6050_Read_Accel(&(gyro_accel->accel));
    // COM_DEBUG_PRINTFln(":%d,%d,%d\n",MPU6050_Acc_Offset[3],MPU6050_Acc_Offset[4],MPU6050_Acc_Offset[5]);
    // 对陀螺仪和加速度计进行校准
    gyro_accel->gyro.x -= MPU6050_Acc_Offset[0];
    gyro_accel->gyro.y -= MPU6050_Acc_Offset[1];
    gyro_accel->gyro.z -= MPU6050_Acc_Offset[2];

    gyro_accel->accel.x -= MPU6050_Acc_Offset[3];
    gyro_accel->accel.y -= MPU6050_Acc_Offset[4];
    gyro_accel->accel.z -= MPU6050_Acc_Offset[5];
    // COM_DEBUG_PRINTFln(":%d,%d,%d",gyro_accel->accel.x,gyro_accel->accel.y,gyro_accel->accel.z);
}

void Int_MPU6050_Calibration(void)
{
    // 先对陀螺仪的加速度进行校准
    Accel_Struct last_accel = {0};
    Accel_Struct accel = {0};
    Int_MPU6050_Read_Accel(&last_accel);
    vTaskDelay(2);
    uint8_t count = 0;
    while (count < 200)
    {
        Int_MPU6050_Read_Accel(&accel);
    
        if (ABS(accel.x - last_accel.x) < 500 && ABS(accel.y - last_accel.y) < 500 && ABS(accel.z - last_accel.z) < 500)
        {

            count++;
        }
        else
        {
            count = 0;
        }

        last_accel = accel;
        // printf(":%d,%d,%d\n", accel.x, accel.y, accel.z);
        vTaskDelay(2);
    }

    // 对陀螺仪进行校准
    GyroAccel_Struct gyroaccel_struct = {0};
    // 用数组记录陀螺仪的偏差值
    int32_t gyroaccel_buff[6] = {0};
    for (int i = 0; i < 200; i++)
    {

        Int_MPU6050_Read_Gyro_Accel(&gyroaccel_struct);
        gyroaccel_buff[0] += gyroaccel_struct.gyro.x - 0;
        gyroaccel_buff[1] += gyroaccel_struct.gyro.y - 0;
        gyroaccel_buff[2] += gyroaccel_struct.gyro.z - 0;

        gyroaccel_buff[3] += gyroaccel_struct.accel.x - 0;
        gyroaccel_buff[4] += gyroaccel_struct.accel.y - 0;
        gyroaccel_buff[5] += gyroaccel_struct.accel.z - 16384;

         vTaskDelay(2);
    }

    // 计算平均值
    MPU6050_Acc_Offset[0] = gyroaccel_buff[0] / 200;
    MPU6050_Acc_Offset[1] = gyroaccel_buff[1] / 200;
    MPU6050_Acc_Offset[2] = gyroaccel_buff[2] / 200;

    MPU6050_Acc_Offset[3] = gyroaccel_buff[3] / 200;
    MPU6050_Acc_Offset[4] = gyroaccel_buff[4] / 200;
    MPU6050_Acc_Offset[5] = gyroaccel_buff[5] / 200;
}
